#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <linux/videodev2.h>
#include <sys/ioctl.h>
#include <assert.h>
#include <assert.h>  
//#include <utils/Log.h>  
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>

#include "camera.h"
#include "jpeglib.h"
#include "myjni.h"
//#include "jpeglib.h"   
#define LOG LOGE
#define LOGV LOGE
//#define ANDROID_ENV
#define CAMERA_DEVICE "/dev/video4"
#define CAPTURE_FILE "/data/local/tmp/frame.yuv420"
#define FRAME_SIZE ((VIDEO_WIDTH*VIDEO_HEIGHT*3)>>1)
#define VIDEO_FORMAT V4L2_PIX_FMT_YUV420    //这里是输出的数据格式，要是摄像头支持jpeg的，可以直接把数据
//V4L2_PIX_FMT_MJPEG         //保存为jpg格式的图片，如果是yuv的则需要转换，转换在下篇介绍。
//V4L2_PIX_FMT_JPEG			   //这里主要是把yuv格式保存为文件
//V4L2_PIX_FMT_YUYV
//V4L2_PIX_FMT_YUV420
//V4L2_PIX_FMT_RGB32


int VIDEO_WIDTH = 720;
int  VIDEO_HEIGHT = 576;
struct fimc_buffer {
    int length;
    void *start;
} framebuf[BUFFER_COUNT];
int fd;
int write_JPEG_file (const char * filename, unsigned char* yuvData, int quality,int image_width,int image_height);

int getTVINSystemType(int fd)
{
    struct v4l2_format format;
    struct v4l2_format format_set;
    struct v4l2_frmsizeenum frmsize;
    enum v4l2_buf_type type;
    int i = 0;   
    int temp_height = 0; 
    memset(&format, 0, sizeof(struct v4l2_format));
    memset(&format_set, 0, sizeof(struct v4l2_format));
    memset(&frmsize, 0, sizeof(struct v4l2_frmsizeenum));   
    frmsize.pixel_format = V4L2_PIX_FMT_NV21;   
    frmsize.index = 0;     
    while ((!ioctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize))&&(i < 20)) 
    {
        //LOGV("i = %d\n", i);     
        //LOGV("framesize width = %d, height = %d\n",frmsize.discrete.width, frmsize.discrete.height);    
        i++;
        frmsize.index++;    
    }    
    memset(&format, 0, sizeof(struct v4l2_format)); 
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    i = 0; 
    while (ioctl(fd, VIDIOC_G_FMT, &format) &&(i++ < 20)) 
    {     
        LOGV("get tvin signal failed.\n");     
        return -1;
    }    
    //LOGV("---width= [%d], height =[%d],format.fmt.raw_data[9]=%d\n",format.fmt.pix.width, format.fmt.pix.height,format.fmt.raw_data[9]);
    format_set.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    format_set.fmt.pix.pixelformat = V4L2_PIX_FMT_NV21;

    format_set.fmt.pix.width= format.fmt.pix.width;
    format_set.fmt.pix.height= format.fmt.pix.height;
    if (ioctl(fd, VIDIOC_S_FMT, &format_set) == -1) {
        LOGV("set tvin image format failed\n");
        return -1 ; 
    }    
    format_set.fmt.raw_data[9]=format.fmt.raw_data[9];
    if((format_set.fmt.raw_data[9]==0))
    {    
        LOGV("------------get tvin NTSC\n");    
        return TVD_NTSC;
    }    
    else if((format_set.fmt.raw_data[9]==1))
    {    
        LOGV("------------get tvin PAL\n");
        return  TVD_PAL ;
    }    
    else if((format_set.fmt.raw_data[9]==2)||(format_set.fmt.raw_data[9]==3))
    {
        LOGV("---------get tvin ypbpr\n");
        return   TVD_YPBPR;
    }
    LOGV("---------get no type \n");
    return   0;

}

int v4l2setCaptureParams()
{
    int ret = -1;

    struct v4l2_streamparm params;
    params.parm.capture.timeperframe.numerator = 1; 
    params.parm.capture.timeperframe.denominator = 30;
    params.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    //params.parm.capture.capturemode = V4L2_MODE_IMAGE;
    //params.parm.capture.capturemode = V4L2_MODE_VIDEO;
    params.parm.capture.capturemode = V4L2_MODE_PREVIEW;


    ret = ioctl(fd, VIDIOC_S_PARM, &params);
    if (ret < 0) 
        LOGE("v4l2setCaptureParams failed, %s", strerror(errno));
    else 
        LOGV("v4l2setCaptureParams ok");

    return ret; 
}


int open_camera()
{
    int i, ret;


    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    // Open Device
    fd = open(CAMERA_DEVICE, O_RDWR | O_NONBLOCK, 0);
    if (fd < 0) {
        LOGE("Open %s failed error=%s\n", CAMERA_DEVICE, strerror(errno));
        return -1;
    }
    // Query Capability
    struct v4l2_capability cap;
    ret = ioctl(fd, VIDIOC_QUERYCAP, &cap);
    if (ret < 0) {
        LOG("VIDIOC_QUERYCAP failed (%d)\n", ret);
        goto out;
    }
    LOGE("video driver:%s", (char *)cap.driver);

    ret = getTVINSystemType(fd);
    if (ret == TVD_PAL) {
        VIDEO_HEIGHT = 576;
    } else if (ret == TVD_NTSC) {
        VIDEO_HEIGHT = 480;
    }
#if 0
    struct v4l2_input inp;
    inp.index = 4;
    if (-1 == ioctl (fd, VIDIOC_S_INPUT, &inp)) {
        LOGE("VIDIOC_S_INPUT fail err=%s", strerror(errno));
    }
#endif
    struct v4l2_format fmt;

    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 
    fmt.fmt.pix.width  = VIDEO_WIDTH; 
    fmt.fmt.pix.height = VIDEO_HEIGHT; 
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_NV21; 
    fmt.fmt.pix.field = V4L2_FIELD_NONE;

    ret = ioctl(fd, VIDIOC_TRY_FMT, &fmt);
    if (ret < 0) 
    {    
        LOGE("VIDIOC_TRY_FMT Failed: %s", strerror(errno));
        return ret; 
    }    


    v4l2setCaptureParams();

    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 
    fmt.fmt.pix.width  = VIDEO_WIDTH; 
    fmt.fmt.pix.height = VIDEO_HEIGHT; 
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_NV21; 
    fmt.fmt.pix.field = V4L2_FIELD_NONE;

    ret = ioctl(fd, VIDIOC_S_FMT, &fmt);
    if (ret < 0) 
    {    
        LOGE("VIDIOC_S_FMT Failed: %s", strerror(errno));
        return ret; 
    }    


    // Request buffers
    struct v4l2_requestbuffers reqbuf;
    reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    reqbuf.memory = /*V4L2_MEMORY_USERPTR;*/V4L2_MEMORY_MMAP;
    reqbuf.count = BUFFER_COUNT;
    ret = ioctl(fd , VIDIOC_REQBUFS, &reqbuf);
    if(ret < 0) {
        LOGE("VIDIOC_REQBUFS failed (%d)\n", ret);
        goto out;
    }
#if 0
    ALOGE("VIDIOC_REQBUFS success");
    //return -1;
    int page_size = getpagesize();
    for (i = 0; i < BUFFER_COUNT; i++) {
        int len =  (FRAME_SIZE + page_size - 1) & ~(page_size - 1); 
        framebuf[i].length = FRAME_SIZE;
        framebuf[i].start = (void *)memalign((size_t)page_size, (size_t)len);
        if (framebuf[i].start == NULL) {
            ALOGE("request memery fail i=%d", i);
            return -1;
        }
        LOG("Frame buffer %d: address=0x%x, length=%d\n", i, (unsigned int)framebuf[i].start, framebuf[i].length);
    }


    for(i = 0; i < BUFFER_COUNT; i++)  
    {  
        struct v4l2_buffer buf;  

        memset(&buf, 0, sizeof(buf));
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;   
        buf.memory = V4L2_MEMORY_USERPTR;     
        buf.index = i;    
        buf.m.userptr = (unsigned long)framebuf[i].start;  
        buf.length = framebuf[i].length;
        LOG("cam qbuf:%d,userptr:0x%08x,length:%d\n", i, buf.m.userptr, buf.length);  
        if(-1 == ioctl(fd,VIDIOC_QBUF,&buf))  
        {  
            LOG("cam Fail to ioctl 'VIDIOC_QBUF'");  
            return -1;
        }  
    }  
    return -1;
#else
    // Queen buffers
    struct v4l2_buffer buf;
    for(i=0; i<BUFFER_COUNT; i++) {
        // Query buffer
        buf.index = i;
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        ret = ioctl(fd , VIDIOC_QUERYBUF, &buf);
        if(ret < 0) {
            LOG("VIDIOC_QUERYBUF (%d) failed (%d)\n", i, ret);
            goto out;
        }
        // mmap buffer
        framebuf[i].length = buf.length;
        framebuf[i].start = (char *) mmap(0, buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
        if (framebuf[i].start == MAP_FAILED) {
            LOG("mmap (%d) failed: %s\n", i, strerror(errno));
            goto out;
        }
#if 1	// Queen buffer
        ret = ioctl(fd , VIDIOC_QBUF, &buf);
        if (ret < 0) {
            LOG("VIDIOC_QBUF (%d) failed (%d)\n", i, ret);
            goto unmap;
        }
#endif
        //	LOG("Frame buffer %d: address=0x%x, length=%d\n", i, (unsigned int)framebuf[i].start, framebuf[i].length);
    }
#endif
#if 1
    ret = ioctl(fd, VIDIOC_STREAMON, &type);
    if (ret < 0) {
        LOG("VIDIOC_STREAMON failed (%d)\n", ret);
        goto unmap;
    }
#endif
#if 0
    // Get frame
    ret = ioctl(fd, VIDIOC_DQBUF, &buf);
    if (ret < 0) {
        LOG("	 (%d)\n", ret);
        return ret;
    }

    printf("%d\n", __LINE__);
    // Process the frame

    FILE *fp = fopen(CAPTURE_FILE, "wb");
    if (fp < 0) {
        LOG("open frame data file failed\n");
        return -1;
    }
    fwrite(framebuf[buf.index].start, 1, buf.length, fp);  
    fclose(fp);

    LOG("Capture one frame saved in %s\n", CAPTURE_FILE);

    //write_JPEG_file("test.jpg", framebuf[buf.index].start, 100, 720, 625); 

    LOG("Capture one frame saved in test.jpg\n");
#endif


    //	run();
    // Re-queen buffer
#if 0
    ret = ioctl(fd, VIDIOC_QBUF, &buf);
    if (ret < 0) { 
        LOG("VIDIOC_QBUF failed (%d)\n", ret);
        return ret;
    }
#endif
    // Release the resource
    //    ALOGE("open camera success");
    return 0;
unmap:
    for (i=0; i<BUFFER_COUNT; i++) {
        munmap(framebuf[i].start, framebuf[i].length);
    }
out:
    close(fd);
    return -1;
}

int close_camera() {
    int i;
    enum v4l2_buf_type  v4l2type; 
    v4l2type = V4L2_BUF_TYPE_VIDEO_CAPTURE;  

    int ret = ioctl(fd, VIDIOC_STREAMOFF, &v4l2type);
    if (ret < 0)
        ALOGE("StopStreaming: Unable to stop capture: %s", strerror(errno));

    for (i=0; i<BUFFER_COUNT; i++) {
        munmap(framebuf[i].start, framebuf[i].length);
    }

    close(fd);
    //	LOG("Camera close Done.\n");
    return 0;
}
static int read_frame (unsigned char *data)
{
    struct v4l2_buffer buf;
    unsigned int i;
    int ret = -1;

    // CLEAR (buf);
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    if (-1 == ioctl (fd, VIDIOC_DQBUF, &buf)) {
        switch (errno) {
            case EAGAIN:
                return -1;
            case EIO:

            default:
                printf("VIDIOC_DQBUF");
        }
    }

    //assert (buf.index < BUFFER_COUNT);
    //assert (buf.field ==V4L2_FIELD_NONE);
    //    process_image (framebuf[buf.index].start);
    if (-1 == ioctl (fd, VIDIOC_QBUF, &buf)) {
        printf ("VIDICO_QBUF error");
        ret = -1;
    } else {
        memcpy(data, framebuf[buf.index].start, buf.length);
        //ALOGE("time:sec=%ld,usec=%ld", buf.timestamp.tv_sec, buf.timestamp.tv_usec);
        ret = buf.length;
    }


    return ret;
}
int get_picture (unsigned char *data)
{
    int ret = -1;
    int i;
    for (i = 0; i < 3; i++) {
        fd_set fds;
        struct timeval tv;
        int r;
        FD_ZERO (&fds);
        FD_SET (fd, &fds);
        tv.tv_sec = 2;
        tv.tv_usec = 0;

        r = select (fd+1, &fds, NULL, NULL, &tv);

        if (-1 == r) {
            if (EINTR == errno)
                continue;
        }

        if (0 == r) {
            LOGE ( "select timeout\n");
            continue;
            //return -1;
            // exit (-1);
        }
        if (FD_ISSET(fd, &fds)) {
            //printf("frame\n");
            ret = read_frame(data);
        }
    }

    return ret;

}

int yuv422_to_yuv420p(char* src, char *dest, int w, int h) {
    int i, j = 0;
    int width = w;
    int height = h;
    char *y = dest;
    char *u = dest + (width * height);
    char *v = dest + (width * height * 5/4);

    int index_y = 0;
    int index_u = 0;
    int index_v = 0;
#if 0
    for(j=0; j< height*2; j++)
    {
        for(i=0; i<width; i=i+4)
        {
            *(y + (index_y++)) = *(src + width * j + i);
            *(y + (index_y++)) = *(src + width * j + i + 2);
            if(j%2 == 0)
            {
                *(v + (index_u++)) = *(src + width * j + i + 1);
                *(u + (index_v++)) = *(src + width * j + i + 3);
            } else {
            }
        }
    }
#endif
    for (i = 0; i < w*h*2; i += 4) {
        y[index_y++] = src[i+0];
        y[index_y++] = src[i+2];
        if (j%2 == 0) {
            v[index_u++] = src[i+1]; //u
            u[index_v++] = src[i+3]; //v
        } else {
        }
        j++;
    }

#if 1
    static int count = 0;
    char path[48];

    sprintf(path, "/sdcard/yuv422%d.yuv", ++count);
    FILE *fp = fopen(path, "wb");
    if (fp == NULL) {
        LOGE("open frame data file failed\n");
        return -1;
    }
    fwrite(src, 1, width*height*2, fp);
    fclose(fp);


    sprintf(path, "/sdcard/yuv420%d.yuv", count);
    fp = fopen(path, "wb");
    if (fp == NULL) {
        LOGE("open frame data file failed\n");
        return -1;
    }
    fwrite(dest, 1, width*height*3/2, fp);
    fclose(fp);
#endif

    return 0;
}

unsigned long yuv420p_to_jpeg(char* yuv_input, unsigned char **jpeg_output, int image_width,int image_height, int quality)  
{     
    struct jpeg_compress_struct cinfo;    
    struct jpeg_error_mgr jerr;    
    cinfo.err = jpeg_std_error(&jerr);    
    jpeg_create_compress(&cinfo);    

    //FILE * outfile;    // target file    
    //if ((outfile = fopen(filename, "wb")) == NULL) {    
    //    LOGE("can't open %s\n", filename);    
    //    return -1;
    //}    
    //jpeg_stdio_dest(&cinfo, outfile);    
    unsigned long jpeg_len;

    jpeg_mem_dest(&cinfo, jpeg_output, &jpeg_len);    

    cinfo.image_width = image_width;  // image width and height, in pixels    
    cinfo.image_height = image_height;    
    cinfo.input_components = 3;    // # of color components per pixel    
    cinfo.in_color_space = JCS_YCbCr;  //colorspace of input image    
    jpeg_set_defaults(&cinfo);    
    jpeg_set_quality(&cinfo, quality, TRUE );    

    //////////////////////////////    
    //  cinfo.raw_data_in = TRUE;    
    cinfo.jpeg_color_space = JCS_YCbCr;    
    cinfo.comp_info[0].h_samp_factor = 2;    
    cinfo.comp_info[0].v_samp_factor = 2;    
    /////////////////////////    

    jpeg_start_compress(&cinfo, TRUE);    

    JSAMPROW row_pointer[1];  

    unsigned char *yuvbuf;  
    if((yuvbuf=(unsigned char *)malloc(image_width*3))!=NULL)  
        memset(yuvbuf,0,image_width*3);  

    char *ybase,*ubase, *vbase;  
    ybase=yuv_input;  
    ubase=yuv_input+image_width*image_height;    
    vbase=ubase + (image_width*image_height)/4;    
    int j=0;  
    while (cinfo.next_scanline < cinfo.image_height)   
    {  
        int idx=0, i = 0;  
        for(i=0;i<image_width;i++)  
        {   
            //转换后rgb颜色不对(偏蓝),调换下面uv的顺序即可
#if 1
            yuvbuf[idx++]=ybase[i + j * image_width];  
            yuvbuf[idx++]=ubase[j/2 * image_width+(i/2)*2+1];   
            yuvbuf[idx++]=ubase[j/2 * image_width+(i/2)*2];  
#else
            //yyyyyy ... uuuu ...vvv  
            yuvbuf[idx++]=ybase[i + j * image_width];  
            yuvbuf[idx++]=ubase[j/2 * image_width/2+(i/2)];    
            yuvbuf[idx++]=vbase[j/2 * image_width/2+(i/2)];  
#endif
        }  
        row_pointer[0] = yuvbuf;  
        jpeg_write_scanlines(&cinfo, row_pointer, 1);  
        j++;  
    }  
    jpeg_finish_compress(&cinfo);    
    jpeg_destroy_compress(&cinfo);    
    //fclose(outfile);  
    free(yuvbuf);

    return jpeg_len;    
}  
